// Sensor Sonar 
// 2010 - Interaction Source - Andre Veloso, Koji Pereira e Marcos Machado
// www.interactionsource.com

//--- Projeto Domus
// interactionsource.com
// Koji Pereira, Andre Veloso e Marcos Paulo

/*
 Projeto Domus
 interactionsource.com
 Koji Pereira, Andre Veloso e Marcos Paulo

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 3 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
*/


// Utilizando ideias de:
// read values from a LV-MaxSonar-EZ1 sensor
// this is for the Arduino MINI -- change the pin values to suit your board.
// jet@flatline.net
// 1 Dec 2007
// e
// http://arduino.cc/en/Tutorial/SerialCallResponse

// nessa versao o dado de distancia em centimetros eh enviado via serial
// utilizando 3 sensores

#ifndef DEBUGACAO
//#define DEBUGACAO  //Descomente a linha para debugacao via serial
#endif


#ifndef ANALOGICO
//#define ANALOGICO  //Descomente a linha para leitura analogica dos dados
#endif

//Output
int statusLed = 11;

//intput - 3 sensores for analog input
int ez1Analog = 0;
int ez2Analog = 1;
int ez3Analog = 2;

//input - for Digtal input
int ez1Digital = 7;
int ez2Digital = 8;
int ez3Digital = 9;

long anVolt1, anVolt2, anVolt3; //for analog input

long pulse1, pulse2, pulse3; //for Digtal input

float inches1, inches2, inches3;
float cm1, cm2, cm3;

float volt1, volt2, volt3;

int val1, val2, val3;

int sum1, sum2, sum3 = 0;    //Create sum variable so it can be averaged, for analog input
int avgrange = 30;  //Quantity of values to average (sample size), for analog input

float smoothedVal1, smoothedVal2, smoothedVal3 = 0.0f;    // smoothed result, for Digtal input

int inByte = 0;         // incoming serial byte

void setup() {
  //pinMode(statusLed,OUTPUT);
  
  pinMode(ez1Analog, INPUT);
  pinMode(ez2Analog, INPUT);
  pinMode(ez3Analog, INPUT);
  
  //analogReference(DEFAULT);
  //analogReference(EXTERNAL);
  
  Serial.begin(9600);
  
  //Comente essa liha para debugacao via serial
  #ifndef DEBUGACAO
  establishContact();  // send a byte to establish contact until receiver responds 
  #endif 
}

void loop() {
 
  #ifndef DEBUGACAO
  if (Serial.available() > 0) {
  #endif  
     
    // get incoming byte:
    inByte = Serial.read();
    
    #ifdef ANALOGICO
    
      #ifdef DEBUGACAO
        Serial.print(" ANALOGICO ");
      #endif 
    
    //----------------------------------------------------------------------------------------
    //Inicio leitura analogica    
    //Realiza a suavizacao de amostras    
    for(int i = 0; i < avgrange ; i++)
    {
      //Used to read in the analog voltage output that is being sent by the MaxSonar device.
      //Scale factor is (Vcc/512) per inch. A 5V supply yields ~9.8mV/in
      anVolt1 = analogRead(ez1Analog);
      sum1 += anVolt1;
      
      anVolt2 = analogRead(ez2Analog);
      sum2 += anVolt2; 

      anVolt3 = analogRead(ez3Analog);
      sum3 += anVolt3;       
      
      delay(10);
    }  
        
    volt1 = sum1/avgrange;
    volt2 = sum2/avgrange;
    volt3 = sum3/avgrange;
    
    //transforma o valor lido em dado de voltagem, para melhor conversao
    volt1 = volt1 * 5.0/1024 ;
    volt2 = volt2 * 5.0/1024 ;
    volt3 = volt3 * 5.0/1024 ;
      
    //transforma a voltagem lida em distancia em polegadas
    //de acordo com a especificacao do sensor  
    inches1 = (volt1/0.0098) ;
    inches2 = (volt2/0.0098) ;
    inches3 = (volt3/0.0098) ;
    
    cm1 = inches1 * 2.54;
    cm2 = inches2 * 2.54;
    cm3 = inches3 * 2.54;
         
    //reset sample total
    sum1 = 0;
    sum2 = 0;
    sum3 = 0;
    
    val1 = map(cm1, 0, 300, 0, 255);
    val2 = map(cm2, 0, 300, 0, 255);
    val3 = map(cm3, 0, 300, 0, 255);
    
    val1 = constrain(val1, 0, 255);
    val2 = constrain(val2, 0, 255);
    val3 = constrain(val3, 0, 255);
    //----------------------------------------------------------------------------------------
    //FIM leitura analogica 
    #endif
    
    
    #ifndef ANALOGICO
    
      #ifdef DEBUGACAO
        Serial.print(" DIGITAL ");
      #endif
    //----------------------------------------------------------------------------------------
    //Inicio leitura Digital 
    //Used to read in the pulse that is being sent by the MaxSonar device.
    //Pulse Width representation with a scale factor of 147 uS per Inch.
    
    pinMode(ez1Digital, INPUT);
    pinMode(ez2Digital, INPUT);
    pinMode(ez3Digital, INPUT);
    
    //pulse1 = pulseIn(ez1Digital, HIGH); - Debugacao 1 sinal somente
    
    pulse1 = pulseIn(ez1Digital, HIGH, 50000);  //50000 - timeout de leitura do pulseIn, em microseconds
    pulse2 = pulseIn(ez2Digital, HIGH, 50000);
    pulse3 = pulseIn(ez3Digital, HIGH, 50000);
    
    //147uS per inch
    inches1 = pulse1/147;
    inches2 = pulse2/147;
    inches3 = pulse3/147;
    
    //change inches to centimetres
    cm1 = inches1 * 2.54;
    cm2 = inches2 * 2.54;
    cm3 = inches3 * 2.54;
      
    smoothedVal1 = smoothedVal1 + ((cm1 - smoothedVal1)/2); // 4 is amount of samples, for Digtal input
    smoothedVal2 = smoothedVal2 + ((cm2 - smoothedVal2)/2);
    smoothedVal3 = smoothedVal3 + ((cm3 - smoothedVal3)/2);
     
    val1 = map(smoothedVal1, 0, 300, 0, 255);
    val2 = map(smoothedVal2, 0, 300, 0, 255);
    val3 = map(smoothedVal3, 0, 300, 0, 255);
    
    val1 = constrain(val1, 0, 255);
    val2 = constrain(val2, 0, 255);
    val3 = constrain(val3, 0, 255);
    
    //----------------------------------------------------------------------------------------
    //FIM leitura Digital 
    #endif
    
    #ifndef DEBUGACAO
    //Envia dado de distancia para a saida serial
    Serial.print(val1,BYTE);
    Serial.print(val2,BYTE);
    Serial.print(val3,BYTE);
    #endif
    
    //blinkLed(statusLed, 10, 255-val);
    
    #ifdef DEBUGACAO
    //Debugacao - comente para a execucao normal do programa
    Serial.print(inches1);
    Serial.print(" in1, ");
    Serial.print(cm1);
    Serial.print(" cm1, ");
    Serial.print(val1);
    Serial.print(" val1, ");  
    Serial.print(smoothedVal1 );
    Serial.print(" smoothedVal1 - "); 

    Serial.print(inches2);
    Serial.print(" in2, ");
    Serial.print(cm2);
    Serial.print(" cm2, ");
    Serial.print(val2);
    Serial.print(" val2, ");
    Serial.print(smoothedVal2 );
    Serial.print(" smoothedVal2 - ");
    
    Serial.print(inches3);
    Serial.print(" in3, ");
    Serial.print(cm3);
    Serial.print(" cm3, ");
    Serial.print(val3);
    Serial.print(" val3, ");
    Serial.print(smoothedVal3 );
    Serial.print(" smoothedVal3 - ");
    
    Serial.println();  
    #endif
    
    delay(100); // delay before loop
  
  #ifndef DEBUGACAO  
  } //Comente o fim do for para debugacao via serial
  #endif
}
  
void establishContact() {
   while (Serial.available() <= 0) {
     Serial.print('A', BYTE);   // send a capital A
     delay(300);
   }
}  
  
  
void blinkLed(int pin, int ms, int val) {
  analogWrite(pin, val); 
  delay(ms);
}


